#!/usr/bin/env python

import sys, rospy, cv2, time
import numpy as np


from Image import RGBD_Image


class Detection:
	def __init__(self):
		self.image_handle = RGBD_Image()

	
	def get_position(self):
		src = self.image_handle.get_image()
		
		# 模板匹配，获得目标的位置
		template = cv2.imread('/home/daisy/catkin_test/src/book.jpeg')
		h, w = template.shape[:2]
		res = cv2.matchTemplate(src, template, cv2.TM_SQDIFF)
		min_val, max_val, min_loc, max_loc = cv2.minMaxLoc(res)
		top_left = min_loc
		bottom_right = (top_left[0] + w, top_left[1] + h)
		
		avg_w = int((top_left[0] + bottom_right[0])/2)
		avg_h = int((top_left[1] + bottom_right[1])/2)
		
		# 显示目标方框图
		cv2.rectangle(src, top_left, bottom_right, (0,0,255), 2)
		cv2.circle(src, (avg_w, avg_h), 10, (0,0,255), 3)
		cv2.waitKey(50)
		cv2.imshow('dst',src)
		
		#print(top_left)
		#print(bottom_right)
		
		dep = self.image_handle.get_depth_image()
		temp = dep[avg_h, avg_w]
		
		return avg_w, avg_h, temp


if __name__ == '__main__':
	detection = Detection()
	rospy.init_node('Detection', anonymous=False)
	while not rospy.is_shutdown():
		x, y, z = detection.get_position()
		print(x, y, z)
		if 27 == cv2.waitKey(1):
			break

